#include <linux/sched.h>
#include <linux/hdreg.h>

#define REALLY_SLOW_IO
#include <asm/io.h>

#include "blk.h"

#define CMOS_READ(addr) ({ \
    outb_p(0x80 | addr, 0x70); \
    inb_p(0x71); \
})

#define MAX_HD 2

struct hd_i_struct {
    int head, sect, cyl, wpcom, lzone, ctl;
};

struct hd_i_struct hd_info[] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
static int NR_HD = 0;

static struct hd_struct {
    long start_sect;
    long nr_sects;
} hd[5 * MAX_HD] = {
    {0, 0},
};

#define port_read(port, buf, nr) \
    __asm__("cld\n\trep\n\tinsw" ::"d"(port), "D"(buf), "c"(nr))

extern void hd_interrupt(void);

int sys_setup(void *BIOS) {
    static int callable = 1;
    int i, drive;
    unsigned char cmos_disks;

    if (!callable)
        return -1;
    callable = 0;

    for (drive = 0; drive < 2; drive++) {
        hd_info[drive].cyl = *(unsigned short *)BIOS;
        hd_info[drive].head = *(unsigned char *)(2 + BIOS);
        hd_info[drive].wpcom = *(unsigned short *)(5 + BIOS);
        hd_info[drive].ctl = *(unsigned char *)(8 + BIOS);
        hd_info[drive].lzone = *(unsigned short *)(12 + BIOS);
        hd_info[drive].sect = *(unsigned char *)(14 + BIOS);
        BIOS += 16;
    }
    if (hd_info[1].cyl)
        NR_HD = 2;
    else
        NR_HD = 1;

    for (i = 0; i < NR_HD; i++) {
        hd[i * 5].start_sect = 0;
        hd[i * 5].nr_sects = hd_info[i].head *
                             hd_info[i].sect * hd_info[i].cyl;
    }

    if ((cmos_disks = CMOS_READ(0x12)) & 0xf0)
        if (cmos_disks & 0x0f)
            NR_HD = 2;
        else
            NR_HD = 1;
    else
        NR_HD = 0;
    for (i = NR_HD; i < 2; i++) {
        hd[i * 5].start_sect = 0;
        hd[i * 5].nr_sects = 0;
    }
    do_hd_request();
    return (0);
}

static int controller_busy(void);
static int status_ok(void);

static int controller_ready(unsigned int drive, unsigned int head) {
    int retry = 100;

    do {
        if (controller_busy() & BUSY_STAT)
            return 0;
        outb_p(0xA0 | (drive << 4) | head, HD_CURRENT);
        if (status_ok())
            return 1;
    } while (--retry);
    return 0;
}

static int status_ok(void) {
    unsigned char status = inb_p(HD_STATUS);

    if (status & BUSY_STAT)
        return 1;
    if (status & WRERR_STAT)
        return 0;
    if (!(status & READY_STAT))
        return 0;
    if (!(status & SEEK_STAT))
        return 0;
    return 1;
}

static int controller_busy(void) {
    int retries = 100000;
    unsigned char status;

    do {
        status = inb_p(HD_STATUS);
    } while ((status & BUSY_STAT) && --retries);
    return status;
}

static int win_result(void) {
    int i = inb_p(HD_STATUS);

    if ((i & (BUSY_STAT | READY_STAT | WRERR_STAT | SEEK_STAT | ERR_STAT)) == (READY_STAT | SEEK_STAT))
        return (0); /* ok */
    if (i & 1)
        i = inb(HD_ERROR);
    return (1);
}

static void hd_out(unsigned int drive, unsigned int nsect, unsigned int sect,
                   unsigned int head, unsigned int cyl, unsigned int cmd,
                   void (*intr_addr)(void)) {
    register int port asm("dx");

    if (drive > 1 || head > 15)
        panic("Trying to write bad sector");
    if (!controller_ready(drive, head))
        panic("HD controller not ready");
    do_hd = intr_addr;
    outb_p(hd_info[drive].ctl, HD_CMD);
    port = HD_DATA;
    outb_p(hd_info[drive].wpcom >> 2, ++port);
    outb_p(nsect, ++port);
    outb_p(sect, ++port);
    outb_p(cyl, ++port);
    outb_p(cyl >> 8, ++port);
    outb_p(0xA0 | (drive << 4) | head, ++port);
    outb(cmd, ++port);
}

void unexpected_hd_interrupt(void) {
    printk("Unexpected HD interrupt\n\r");
}

char buffer[512] = {0};

static void read_intr(void) {
    if (win_result())
        return;
    port_read(HD_DATA, buffer, 256);
    printk("The data that we read:\n\r");
    for (int i = 0; i < 8; i++) {
        for (int j = 0; j < 8; j++) {
            printk("%d ", buffer[i * 8 + j]);
        }
        printk("\n\r");
    }
    return;
}

void do_hd_request(void) {
    hd_out(0, 1, 3, 0, 0, WIN_READ, &read_intr);
}

void hd_init(void) {
    set_intr_gate(0x2E, &hd_interrupt);
    outb_p(inb_p(0x21) & 0xfb, 0x21);
    outb(inb_p(0xA1) & 0xbf, 0xA1);
}
